/*
 * IMU.cpp
 *
 *  Created on: Jan 14, 2014
 *      Author: ppks
 */

#include "common.h"
#include "IMU.h"
#include "FlightController_Common.h"
#include "Accelerometer.h"
#include "Barometer.h"
#include "Gyroscope.h"
#include "Magnetometer.h"
#include "MathMethods.h"
#include "GPS.h"
#include "Alarms.h"


/******************************************************************************/
/* Global Variables Declaration                                                */
/******************************************************************************/
int16_t accZ = 0;

/******************************************************************************/
/* Local Functions Declaration                                                */
/******************************************************************************/
static void IMU_InitVariablesDefaults(void);
static void IMU_ExecuteTaskInOrder(void);
static void IMU_annexCode(void);
static void IMU_getEstimatedAttitude(void);

/******************************************************************************/
/* Function Code                                                              */
/******************************************************************************/
/*******************************************************************************
* Function Name: IMU_Init()
********************************************************************************
* Summary:
*  Performs sensors initialization
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void IMU_Init(void)
{
    /* Init Default Variables */
    IMU_InitVariablesDefaults();

    /* Wait 200ms */
    delay(200);

    /* Init all components of the IMU board */
#if GYRO_INSTALLED
    Gyroscope_Init();
#endif /* GYRO_INSTALLED */
#if BARO_INSTALLED
    Barometer_Init();
#endif /* BARO_INSTALLED */
#if MAG_INSTALLED
    Magnetometer_Init();
#endif /* MAG_INSTALLED */
#if ACC_INSTALLED
    Accelerometer_Init();
#endif /* ACC_INSTALLED */
}

/*******************************************************************************
* Function Name: IMU_InitVariablesDefaults()
********************************************************************************
* Summary:
*  Initializes the I2C block and configure pull-ups
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
static void IMU_InitVariablesDefaults(void)
{
    /* Initialize default calibration cycles */
    /* Accelerometer */
    if (IsAccCalibrated() == 0)
    {
        calibration.AccCycles = ACC_CALIBRATION_CYCLES_DEFAULT;
    }
    else
    {
        calibration.AccCycles = 0;
    }

    /* Gyroscope */
    if (IsGyroCalibrated() == 0)
    {
        calibration.GyroCycles = GYRO_CALIBRATION_CYCLES_DEFAULT;
    }
    else
    {
        calibration.GyroCycles = 0;
    }

    /* Magnetometer */
    if (IsMagCalibrated() == 0)
    {
        calibration.MagCycles = MAG_CALIBRATION_CYCLES_DEFAULT;
    }
    else
    {
        calibration.MagCycles = 0;
    }

    /* Barometer */
    if (IsBaroCalibrated() == 0)
    {
        calibration.BaroCycles = BARO_CALIBRATION_CYCLES_DEFAULT;
    }
    else
    {
        calibration.BaroCycles = 0;
    }
}


/*******************************************************************************
* Function Name: IMU_getEstimatedAttitude()
********************************************************************************
* Summary:
*  Simplified IMU based on "Complementary Filter"
*  Inspired by http://starlino.com/imu_guide.html
*
*  adapted by ziss_dm : http://www.multiwii.com/forum/viewtopic.php?f=8&t=198
*
*  The following ideas was used in this project:
*  1) Rotation matrix: http://en.wikipedia.org/wiki/Rotation_matrix
*  2) Small-angle approximation: http://en.wikipedia.org/wiki/Small-angle_approximation
*  3) C. Hastings approximation for atan2()
*  4) Optimization tricks: http://www.hackersdelight.org/
*
*  Currently Magnetometer uses separate CF which is used only
*  for heading approximation.
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
uint16_t deltaT;

static void IMU_getEstimatedAttitude(void)
{
    uint8_t axis;
    int32_t accMag = 0;
    int32_t accZ_tmp = 0;
    int16_t deltaGyroAngle16[3];
    float invG; /* 1/|G| */
    float scale;
    static t_int32_t_vector EstG;
    static t_int32_t_vector EstM;
    static uint32_t LPFAcc[3];
    static int16_t accZoffset = 0;
    static uint16_t previousT;
    uint16_t currentT = micros();

    /* unit: radian per bit, scaled by 2^16 for further multiplication */
    /* with a delta time of 3000 us, and GYRO scale of most gyros, scale = a little bit less than 1 */
    deltaT = currentT - previousT;
    scale = (currentT - previousT) * (GYRO_SCALE * 65536);
    previousT = currentT;

    /* Initialization */
    for (axis = 0; axis < 3; axis++)
    {
        /* valid as long as LPF_FACTOR is less than 15 */
        imuData.accSmooth[axis]  = LPFAcc[axis] >> ACC_LPF_FACTOR;
        LPFAcc[axis]      += imuData.accADC[axis] - imuData.accSmooth[axis];

        /* used to calculate later the magnitude of acc vector */
        accMag   += mul(imuData.accSmooth[axis] , imuData.accSmooth[axis]);
        /* unit: radian scaled by 2^16 */
        /* imu.gyroADC[axis] is 14 bit long, the scale factor ensure deltaGyroAngle16[axis] is still 14 bit long */
        deltaGyroAngle16[axis] = imuData.gyroADC[axis]  * scale;
    }

    /* we rotate the intermediate 32 bit vector with the radian vector (deltaGyroAngle16), scaled by 2^16
     * however, only the first 16 MSB of the 32 bit vector is used to compute the result
     * it is ok to use this approximation as the 16 LSB are used only for the complementary filter part */
    rotateV32(&EstG,deltaGyroAngle16);
#if MAG_INSTALLED
    rotateV32(&EstM,deltaGyroAngle16);
#endif

    /* Apply complimentary filter (Gyro drift correction)
     * If accel magnitude >1.15G or <0.85G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
     * To do that, we just skip filter, as EstV already rotated by Gyro */
    for (axis = 0; axis < 3; axis++)
    {
        if ( (int16_t)(0.85 * ACC_1G_VALUE * ACC_1G_VALUE / 256) < (int16_t)(accMag >> 8) &&
             (int16_t)(accMag >> 8) < (int16_t)(1.15 * ACC_1G_VALUE * ACC_1G_VALUE / 256) )
        {
            EstG.A32[axis] += (int32_t)(imuData.accSmooth[axis] - EstG.A16[2 * axis + 1]) << (16 - GYR_CMPF_FACTOR);
        }

        accZ_tmp += mul(imuData.accSmooth[axis] , EstG.A16[2 * axis+1]);
    #if MAG_INSTALLED
        EstM.A32[axis]  += (int32_t)(imuData.magADC[axis] - EstM.A16[2 * axis + 1]) << (16 - GYR_CMPFM_FACTOR);
    #endif
    }

    /* Set the global flag is the change is over 25 degree */
    if (EstG.V16.Z > ACCZ_25DEG)
    {
        globalFlags.SMALL_ANGLES_25 = 1;
    }
    else
    {
        globalFlags.SMALL_ANGLES_25 = 0;
    }

    /* Attitude of the estimated vector */
    int32_t sqGX_sqGZ = mul(EstG.V16.X,EstG.V16.X) + mul(EstG.V16.Z,EstG.V16.Z);
    invG = InvSqrt(sqGX_sqGZ + mul(EstG.V16.Y,EstG.V16.Y));
    att.angle[ROLL]  = _atan2(EstG.V16.X , EstG.V16.Z);
    att.angle[PITCH] = _atan2(EstG.V16.Y , InvSqrt(sqGX_sqGZ) * sqGX_sqGZ);

#if MAG_INSTALLED
    /* note on the second term: mathematically there is a risk of overflow (16*16*16=48 bits). assumed to be null with real values */
    att.heading = _atan2(mul(EstM.V16.Z , EstG.V16.X) - mul(EstM.V16.X , EstG.V16.Z),
                         (EstM.V16.Y * sqGX_sqGZ  - (mul(EstM.V16.X , EstG.V16.X) + mul(EstM.V16.Z , EstG.V16.Z)) * EstG.V16.Y)*invG );
    att.heading += FC_Config.MISC.mag_declination; /* Set from GUI */
    att.heading /= 10;
#endif

#if defined(THROTTLE_ANGLE_CORRECTION)
    cosZ = mul(EstG.V16.Z , 100) / ACC_1G_VALUE ;                                               /* cos(angleZ) * 100 */
    throttleAngleCorrection = THROTTLE_ANGLE_CORRECTION * constrain(100 - cosZ, 0, 100) >> 3;   /* 16 bit ok: 200*150 = 30000 */
#endif

    /* projection of ACC vector to global Z, with 1G subtructed */
    /* Math: accZ = A * G / |G| - 1G */
    accZ = accZ_tmp *  invG;
    if (!globalFlags.ARMED)
    {
        accZoffset -= accZoffset >> 3;
        accZoffset += accZ;
    }
    accZ -= accZoffset >> 3;
}

/*******************************************************************************
* Function Name: IMU_Process()
********************************************************************************
* Summary:
*  Process All Sensor Readings Here
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void IMU_Process(void)
{
    static int16_t gyroADCprevious[3] = {0,0,0};
    static int16_t gyroADCinter[3];
    uint8_t axis;

    uint16_t timeInterleave = 0;

    /* Execute current task based on order */
    IMU_ExecuteTaskInOrder();

#if ACC_INSTALLED
    /* Read and update accelerometer data */
    Accelerometer_ReadRawADC();

    /* Estimate the andgles and heading */
    IMU_getEstimatedAttitude();
#endif /* ACC_INSTALLED */

#if GYRO_INSTALLED
    Gyroscope_ReadRawADC();
#endif /* GYRO_INSTALLED */

    /* Save the value of the gyro raw reading */
    for (axis = 0; axis < 3; axis++)
    {
        gyroADCinter[axis] =  imuData.gyroADC[axis];
    }

    timeInterleave = micros();

    /* Data processing for rcCommands */
    IMU_annexCode();

    /* If the IMU_annexCode exited earlier than 650 us - wait here */
    while((int16_t)(micros() - timeInterleave) < 650);

    /* Read gyro data second time for averaging */
#if GYRO_INSTALLED
    Gyroscope_ReadRawADC();
#endif /* GYRO_INSTALLED */

    /* Calculate average data for GYRO */
    for (axis = 0; axis < 3; axis++)
    {
        gyroADCinter[axis] =  imuData.gyroADC[axis] + gyroADCinter[axis];

        /* empirical, we take a weighted value of the current and the previous values */
        imuData.gyroData[axis] = (gyroADCinter[axis] + gyroADCprevious[axis]) / 3;

        gyroADCprevious[axis] = gyroADCinter[axis] >> 1;

        /* Reset accelerometer data if it is not installed */
        if (!ACC_INSTALLED)
        {
            imuData.accADC[axis] = 0;
        }
    }
}

/*******************************************************************************
* Function Name: IMU_ExecuteTaskInOrder()
********************************************************************************
* Summary:
*  Process All Sensor Readings Here
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
static void IMU_ExecuteTaskInOrder(void)
{
    /* never call all functions in the same loop, to avoid high delay spikes */
    static uint8_t taskOrder = 0;

    /* Check if task counter has to be reset */
    if (taskOrder > 4)
    {
        taskOrder = 0;
    }

    /* Run specific routine depending on the task order in current loop */
    switch (taskOrder)
    {
        case 0:     /* Read Magnetometer Data */
                taskOrder++;
            #if MAG_INSTALLED
                if (Magnetometer_GetNewData() != 0 )
                {
                    /* Terminate loop only if new data was available,
                     * otherwise proceed to the next task in order
                     */
                    break;
                }
            #endif /* MAG_INSTALLED */

        case 1:     /* Read BArometer Data */
                taskOrder++;
            #if BARO_INSTALLED
                if (Barometer_UpdateData() != BARO_STATE_NO_NEW_DATA)
                {
                    /* Terminate loop only if new data was available,
                     * otherwise proceed to the next task in order
                     */
                    break; /* for MS baro: I2C set and get: 220 us  -  presure and temperature computation 160 us */
                }
            #endif /* BARO_INSTALLED */

        case 2:     /* Calculate Estimated Altitude */
                taskOrder++;
            #if BARO_INSTALLED
                if (Barometer_GetEstimatedAltitude() !=0)
                {
                    /* Terminate loop only if new data was available,
                     * otherwise proceed to the next task in order
                     */
                    break; /* 280 us */
                }
            #endif /* BARO_INSTALLED */

        case 3:
                taskOrder++;
            #if GPS_INSTALLED
                if (GPS_Enabled)
                {
                	GPS_GetNewData();  /* I2C GPS: 160 us with no new data / 1250us! with new data */
                }
                break;
            #endif

        case 4:
                taskOrder++;
            #if SONAR
                Sonar_update();
            #endif
                break;
        }
}

/*******************************************************************************
* Function Name: IMU_annexCode()
********************************************************************************
* Summary:
*  Process All Sensor Readings Here
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
static void IMU_annexCode(void)
{
    /* this code is executed at each loop and won't interfere with control loop if it lasts less than 650 microseconds */
    static uint32_t calibratedAccTime;
    uint16_t tmp;
    uint16_t tmp2;
    uint8_t axis;
    uint8_t prop1;
    uint8_t prop2;

    /* PITCH & ROLL only dynamic PID adjustment, depending on throttle value */
    prop2 = 128; /* prop2 was 100, is 128 now */
    if (rcData[THROTTLE] > RC_MIDVALUE)
    { /* breakpoint is fix: 1500 */
        if (rcData[THROTTLE] < 2000)
        {
            prop2 -=  ((uint16_t)FC_Config.RC.dynThrPID * (rcData[THROTTLE] - 1500) >> 9);   /*/512 instead of /500 */
        }
        else
        {
            prop2 -=  FC_Config.RC.dynThrPID;
        }
    }

    for(axis = 0; axis < 3; axis++)
    {
        tmp = min(abs(rcData[axis] - RC_MIDVALUE), 500);
    #if defined(DEADBAND)
        if (tmp>DEADBAND)
        {
            tmp -= DEADBAND;
        }
        else
        {
            tmp=0;
        }
    #endif

        if(axis != 2)
        { /* ROLL & PITCH */
            tmp2 = tmp >> 7; /* 500/128 = 3.9  => range [0;3] */
            rcCommand[axis] = lookupPitchRollRC[tmp2] + ((tmp - (tmp2 << 7)) * (lookupPitchRollRC[tmp2 +1 ] - lookupPitchRollRC[tmp2]) >>7 );
            prop1 = 128 - ((uint16_t)FC_Config.RC.rollPitchRate * tmp >> 9); /* prop1 was 100, is 128 now -- and /512 instead of /500 */
            prop1 = (uint16_t)prop1*prop2>>7; /* prop1: max is 128   prop2: max is 128   result prop1: max is 128 */
            dynP8[axis] = (uint16_t)FC_Config.PID[axis].P8 * prop1 >> 7; /* was /100, is /128 now */
            dynD8[axis] = (uint16_t)FC_Config.PID[axis].D8 * prop1 >> 7; /* was /100, is /128 now */
        }
        else
        {      /* YAW */
            rcCommand[axis] = tmp;
        }

        if (rcData[axis] < RC_MIDVALUE)
        {
            rcCommand[axis] = -rcCommand[axis];
        }
    }

    tmp = constrain(rcData[THROTTLE], RC_MINCHECK, 2000);
    tmp = (uint32_t)(tmp - RC_MINCHECK) * 2559 / (2000 - RC_MINCHECK); /* [MINCHECK;2000] -> [0;2559] */
    tmp2 = tmp/256; /* range [0;9] */
    rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 256) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 256; /* [0;2559] -> expo -> [FC_Config.MISC.minthrottle;MAXTHROTTLE] */

    if (globalFlags.HEADFREE_MODE)
    { /* to optimize */
        float radDiff = (att.heading - headFreeModeHold) * 0.0174533f; /* where PI/180 ~= 0.0174533 */
        float cosDiff = cos(radDiff);
        float sinDiff = sin(radDiff);
        int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
        rcCommand[ROLL] =  rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
        rcCommand[PITCH] = rcCommand_PITCH;
    }


    /* Perform Analog Sensor Reading (1 at a time per main loop cycle */
    FlightController_AnalogSensorsRead();

#if defined(BUZZER)
    /* external buzzer routine that handles buzzer events globally now */
    Alarms_ProcessHandler();
#endif /* defined(BUZZER) */

    /* LED handling */
    if ( ((calibration.AccCycles > 0) && ACC_INSTALLED ) || (calibration.GyroCycles > 0) )
    { /* Calibration phase */
        LEDPIN_TOGGLE;
    }
    else
    {
        if (globalFlags.ACCL_CALIBRATED != 0)
        {
            LEDPIN_OFF;
        }

        if (globalFlags.ARMED)
        {
            LEDPIN_ON;
        }
    }

    if ( currentLoopTimeStamp > calibratedAccTime )
    {
        if (! globalFlags.SMALL_ANGLES_25)
        {
            /* the multi uses ACC and is not calibrated or is too much inclined */
            globalFlags.ACCL_CALIBRATED = 0;

            LEDPIN_TOGGLE;
            calibratedAccTime = currentLoopTimeStamp + 100000;
        }
        else
        {
            globalFlags.ACCL_CALIBRATED = 1;
        }
    }
}





